
#include "World.h"
#include "Entity.h"
#include "Collider.h"
#include "ConstraintSolver.h"
#include "BroadPhase.h"

namespace p2 {

IMPLEMENT_RTT(p2, World);

World::~World()
{
}

void World::integrate(real64 th)
{
	real64 dT = Math<real64>::Clamp(th - _t0, 0.0, 0.25);
	_t0 = th;

	_accumulatedT += dT * _timeMultiplier;
	while (_accumulatedT >= _dTu) {
		_accumulatedT -= _dTu;

		/*
		 * REF: http://physbam.stanford.edu/~fedkiw/papers/stanford2003-01.pdf
		 * integrate velocities; using the split integration model
		 */
        foreach (Entity* entity, _entities) {
        	entity->initialise(_dTu);
			entity->addCenterForce(entity->getMass() * _g);
		}

		/*
         * correct position by applying sufficient impulse so that all
		 * constraints will be satisfied within a tolerance after the
		 * next world space update (t0 + h)
		 */
        _constraintSolver.preStep(_dTu, _constraints);

        _constraintSolver.solvePositions(_dTu, _constraints, 3);
		/*
		 * integrate position, update transform, update world model
		 */
        foreach (Entity* entity, _entities) {
			entity->integratePosition(_dTu);
			entity->updateWorldSpace();
		}

         /*
         * REF: http://i31www.ira.uka.de/docs/Paper_VC05.pdf
         * 'After computing an impulse for every position constraint and
         * applying it to the rigid  bodies, the velocities of all bodies
         * change. Now a simulation step .. is made. The new velocities have
         * the effect that all position constraints are satisfied after the
         * simulation step but now in general the velocity constraints of
         * the joints are not satisfied anymore.'
         */
        _constraintSolver.solveVelocities(_dTu, _constraints, 3);

        foreach (Entity* entity, _entities) {
			entity->integrateVelocity(_dTu);
			entity->updateMotionBounds(_dTu);
		}

        /*
 		 * update all objects in broad phase, ask for all overlapping pairs
 		 */
         foreach (Entity* entity, _entities) {
         	entity->updateBroadphaseObjects(_dTu);
         }
 		_collider.getContacts().clear();
 		_broadPhase->updateNotify();

 		_contactSolver.solve(_dTu, _collider.getContacts(), 3);
	}
}

Element* World::elementContains(Vector2<real> p)
{
	/*
	 * reverse iterate since the entities are sorted collision plane ascending
	 */
	Entity::Set::reverse_iterator riter = _entities.rbegin();
    for (; riter != _entities.rend(); ++riter)
    {
    	Element* found = NULL;
        if ( (*riter)->contains(p, found) ) {
            return found;
        }
    }
    return NULL;
}

bool World::addPair(void* data0, void* data1)
{
	Element* c0 = (Element*) data0;
	Element* c1 = (Element*) data1;
	if (c0->getOwner() == c1->getOwner()) {
		/*
		 * REF: http://i31www.ira.uka.de/docs/Paper_VC05.pdf
		 * 'It makes no sense to test collision geometries of the same rigid body for contacts or interpenetration.'
		 */
		return false;
	}
	if (c0->getOwner()->getInvMass()==0.f && c1->getOwner()->getInvMass()==0.f) {
		/*
		 * REF: http://i31www.ira.uka.de/docs/Paper_VC05.pdf
		 * 'A collision test between static bodies is also unnecessary and just consumes time for computation.'
		 */
		return false;
	}
	if (_collider.collide(c0, c1)) {
		return true;
	}
	return false;
}

bool World::removePair(void* data0, void* data1)
{
	UNUSED(data0);
	UNUSED(data1);
	//Collider* A = (Collider*) _singleSAP.getObject( event->id0() )._data;
	//Collider* B = (Collider*) _singleSAP.getObject( event->id1() )._data;
	//std::pair<p2ContactMap::iterator, p2ContactMap::iterator> m(_contacts.equal_range(uniqueId));
	//for ( ; m.first != m.second; ++m.first ) {
	//	delete (*m.first).second;
	//}
	//_contacts.erase(uniqueId);

	return true;
}

}
